import rclpy
import cv2
import cv_bridge
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
import time

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")
        
        self.bridge = cv_bridge.CvBridge()
        
        self.image_sub = self.create_subscription(Image, 'image_raw1', self.image_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.pub = self.create_publisher(Image, '/camera/process_image', 10)
    
        self.twist = Twist()
        self.qrDecoder = cv2.QRCodeDetector()  # Create a qrCodeDetector Object
    
    def image_callback(self, msg):
        self.get_logger().info("Image callback triggered.")
        try:
            image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"CvBridge Error: {e}")
            return
        
        # Detect and decode the QR code
        t = time.time()
        data, bbox, rectifiedImage = self.qrDecoder.detectAndDecode(image)
        if data:
            self.get_logger().info(f"QR Code detected: {data}")
        
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        lower_black = np.array([0, 10, 10])
        upper_black = np.array([255, 50, 90])
        kernel = np.ones((5,5), np.uint8)
        # hsv_dilate = cv2.dilate(hsv, kernel, iterations=1)  # 膨胀操作
        # hsv_erode = cv2.erode(hsv_dilate, kernel, iterations=1)  # 腐蚀操作
        mask = cv2.inRange(hsv, lower_black, upper_black)

        h, w, d = image.shape
        search_top = int(h * 2 / 3)
        search_bot = int(h / 2 + 20)
        mask[0:search_top, 0:w] = 0
        M = cv2.moments(mask)
        
        if M['m00'] > 0:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
            
            # 基于检测的目标中心点，计算机器人的控制参数
            err = cx - w / 2
            self.twist.linear.x = 0.2
            self.twist.angular.z = -float(err) / 380
            self.cmd_vel_pub.publish(self.twist)
        
        try:
            self.pub.publish(self.bridge.cv2_to_imgmsg(image, 'bgr8'))
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"Publish CvBridge Error: {e}")

def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
